#include "include/KLTFlow.h"
#include "include/matching.h"


void KLT::setImageParam(cv::Mat input)
{
  temp = input;
  capture_image = &temp;
  width = capture_image->width;
  height = capture_image->height;

  buf= (unsigned char*)(capture_image->imageData);
  gbuf=(unsigned char *)malloc((int)(width*height));
  ogbuf=(unsigned char *)malloc((int)(width*height));
  bfl = (float **)malloc(sizeof (float*)*fn);
  for(int i=0; i<fn; i++)
    bfl[i]= (float *)malloc(sizeof(float)*2);

  for(int i=0 ; i < width*height ; i++){
    xidx = i*3;
    ogbuf[i]=(unsigned char)(((3*buf[xidx]+6*buf[xidx+1]+buf[xidx+2])/10));
  }
  KLTSelectGoodFeatures(tc,ogbuf,width,height,fl);
}

void KLT::calcKlt(cv::Mat input){
  temp = input;
  capture_image = &temp;
  for (int i=0; i<fn; i++)
    {
      bfl[i][0] = (fl->feature[i]->x);
      bfl[i][1] = (fl->feature[i]->y);
    }

  buf= (unsigned char*)(capture_image->imageData);
  for(int i=0;i<width*height;i++)
    {
      xidx=i*3;
      gbuf[i]=(unsigned char)(((3*buf[xidx]+6*buf[xidx+1]+buf[xidx+2])/10));
    }
  KLTTrackFeatures(tc,ogbuf,gbuf,width,height,fl);
  KLTReplaceLostFeatures(tc,gbuf,width,height,fl);

}

void KLT::getFlowPoint(vector<Point2f> *flowPoint)
{
  if(flowPoint->size() > 0)
    flowPoint->clear();

  Point2f temp;
  for(int i = 0; i < fn ; i++)
    {
      if(fl->feature[i]->val==0.0)
	{
	  temp.x = fl->feature[i]->x;
	  temp.y = fl->feature[i]->y;
	  flowPoint->push_back(temp);
	}
    }
}

void KLT::getFlowScale(vector<Point2f> *flowScale)
{
  if(flowScale->size() > 0)
    flowScale->clear();

  Point2f temp;
  for(int i = 0; i < fn ; i++)
    {
      if(fl->feature[i]->val==0.0)
	{
	  temp.x = fl->feature[i]->x - bfl[i][0];
	  temp.y = fl->feature[i]->y - bfl[i][1];
	  flowScale->push_back(temp);
	}
    }
}

void KLT::setFeatureNum(int num)
{
  KLTFreeFeatureList(fl);
  for(int i=0; i<fn; i++)
    {
      free( bfl[i] );
    }
  free( bfl );
  fn = num;
  fl = KLTCreateFeatureList(fn);
  bfl = (float **)malloc(sizeof (float*)*fn);
  for(int i=0; i<fn; i++)
    bfl[i]= (float *)malloc(sizeof(float)*2);
  KLTSelectGoodFeatures(tc,ogbuf,width,height,fl);

}

void KLT::firstProc(int fnum, cv::Mat srcimage)
{
  setImageParam(srcimage);
  setFeatureNum(fnum);
}



void KLT::Proc(std::string dirname, int imcount, cv::Mat& output,vector<Point2f> &flowPoint, vector<Point2f> &flowScale)
{
  static char *home = getenv("HOME");

  //ターゲットの設定
  string imdir;
  imdir.append(home);
  imdir.append("/Pictures/");
  imdir.append(dirname.c_str());
  imdir.append("/");

  output = loadImage(imdir, imcount);
  resizeSrcImg(output,output, 0.8, INTER_LINEAR);
  calcKlt(output);
  getFlowPoint(&flowPoint);
  getFlowScale(&flowScale);

}
